/*******************************************************************************
Copyright:      BUPT
File name:      chassis.c
Description:    全向轮底盘控制代码
Author:         ZX & ZH & LEO
Version：       1.0
Data:           2019/12/09
*******************************************************************************/
#include "chassis.h"
#include "chassis_handle.h"
#include "laser.h"
#include "point_parser.h"
#include "dji_ctr.h"
#include "karman.h"
#include "placemedicine.h"
// #define MAX_CHASSIS_MOVE_SPEED 500  //! chassis_move中对移动速度做限幅
// #define MAX_CHASSIS_ANGLE_SPEED 350 //! chassis_move中对自转速度做限幅
#define MAX_CHASSIS_MOVE_SPEED 600  //! chassis_move中对移动速度做限幅
#define MAX_CHASSIS_ANGLE_SPEED 100 //! chassis_move中对自转速度做限幅
Chassis chassis;
Chassis_Status chassis_status;
// 500 0 走直线
PID_Struct y_pid = {3000, 125000, 0, 0, 0, 5000, 0, 0.005}; //速度方向控制

PID_Struct angle_pid = {1000, 0, 0, 0, 0, 5000, 0, 0.005}; //偏高角控制

float Arrive_distance = 0.005;
int chassis_start_flag = 0;
int goto_point_speed = 100;
/*****************************初始化*************************/

/**底盘状态初始化*/
void chassis_init_status()
{
  chassis_status.go_to_point = 0; //go_to_point_for_test函数控制变量，1为开启，0为关闭
  chassis_status.count = 0;       //跑点计数，初试为0
  chassis_status.trace_count = 0;
  chassis_status.run_point = 0;
  chassis_status.run_point_test = 0;
  chassis_status.vega_is_ready = 0; //初始化前不ready
  chassis_status.is_begin = 1;
  flag.chassis_control_flag = 0; //控制周期5ms
}
/**底盘位置初始化*/
void chassis_init_pos(float x, float y)
{
  chassis.fspeed = 0;
  chassis.fangle = 0;
  chassis.fturn = 0;
  chassis.vega_init_pos_x += x - chassis.pos_x;
  chassis.vega_init_pos_y += y - chassis.pos_y;
}
/**底盘初始化*/
void chassis_init(void)
{
  chassis.vega_init_pos_x = 0;
  chassis.vega_init_pos_y = 0;
  chassis.vega_init_angle = 0;
  // chassis_init_pos(points_pos0[1].x , points_pos0[1].y);
  chassis_init_pos(3, 0.5);
  chassis_init_status();
}
/****************************计算**************************/
//交换x、y坐标的函数，同时修正0度方向，由于轨迹规划上位机坐标轴与实际坐标轴不符合而使用
//参数：待交换轨迹集，轨迹集内点个数
//author：zohycao
void chassis_swap_xy(Point points_pos[], int point_num)
{
  int i;
  float temp_pos;
  for (i = 0; i < point_num; i++)
  {
    temp_pos = points_pos[i].x;
    points_pos[i].x = points_pos[i].y;
    points_pos[i].y = temp_pos;
    points_pos[i].direct = -points_pos[i].direct;
    points_pos[i].direct += PI / 2;
  }
  return;
}

/**计算到目标点的角度*/
float chassis_calculate_traceangle(float point_x, float point_y)
{
  return Angle_Between_Points(chassis.pos_x, chassis.pos_y, point_x, point_y);
}
/**计算直线跑点速度*/
int chassis_calculate_linespeed(float point_x, float point_y, int start_speed, int final_speed, int max_speed)
{
  float distance_to_target = 1000 * sqrtf((point_x - chassis.pos_x) * (point_x - chassis.pos_x) + (point_y - chassis.pos_y) * (point_y - chassis.pos_y));

  //int int_speed = (int)((start_speed - final_speed)*distance_to_target + final_speed);

  int int_speed = (int)distance_to_target;

  // if(int_speed < 0) int_speed = -int_speed;
  Limit_From_To(int_speed, 0, max_speed);
  return int_speed;
}
/****************************驱动**************************/

/**底盘电机驱动*/
void chassis_canset_motorduty(int s1, int s2, int s3)
{
  can_msg can_TX_data[3];
  can_TX_data[0].in[0] = 0;
  can_TX_data[1].in[0] = 0;
  can_TX_data[2].in[0] = 0;
  can_TX_data[0].in[1] = s1;
  can_TX_data[1].in[1] = s2;
  can_TX_data[2].in[1] = s3;

  can_send_msg(send_id.motor0_id, &can_TX_data[0]);
  can_send_msg(send_id.motor1_id, &can_TX_data[1]);
  can_send_msg(send_id.motor2_id, &can_TX_data[2]);
}
void chassis_canset_motorspeed(int s1, int s2, int s3)
{
  // uprintf("can speed set %d %d %d\r\n", s1, s2, s3);
  // can_msg can_TX_data[3];

  // can_TX_data[0].in[0] = 1;
  // can_TX_data[1].in[0] = 1;
  // can_TX_data[2].in[0] = 1;
  // can_TX_data[0].in[1] = s1;
  // can_TX_data[1].in[1] = s2;
  // can_TX_data[2].in[1] = s3;

  // can_send_msg(send_id.motor0_id, &can_TX_data[0]);
  // can_send_msg(send_id.motor1_id, &can_TX_data[1]);
  // can_send_msg(send_id.motor2_id, &can_TX_data[2]);
  uint16_t vel[4] = {0, s1, s2, s3};
  velCtrAll(vel);
}
float ERR_angle_m2 = 0, ERR_angle_m1 = -PI * 2.0 / 3.0, ERR_angle_m0 = PI * 2.0 / 3.0; //三轮与全场定位模块安装偏角
/**底盘底层驱动(跑速度):speed 速度;direction 速度方向;target_angle 偏航角*/            //TODO: 为什么要用int类型
void chassis_move(int speed, float direction, float target_angle)
{
  Limit(speed, MAX_CHASSIS_MOVE_SPEED);

  float speed_out_0 = -(speed * cos((ERR_angle_m0 + chassis.angle) - direction));
  float speed_out_1 = -(speed * cos((ERR_angle_m1 + chassis.angle) - direction));
  float speed_out_2 = -(speed * cos((ERR_angle_m2 + chassis.angle) - direction));
  float angle_output = 0;
  angle_output = -PID_Release(&angle_pid, target_angle, chassis.angle);
  Limit(angle_output, MAX_CHASSIS_ANGLE_SPEED);
  //此处将angle_output改为了两种，即自动时为偏航角，手动时为自旋角速度，后续需改成根据MODE再判断，
  // if (flag.chassis_auto_flag == 1 && flag.chassis_handle_flag == 0)
  // {
  //   angle_output = -PID_Release(&angle_pid, target_angle, chassis.angle);
  //   Limit(angle_output, MAX_CHASSIS_ANGLE_SPEED);
  // }
  // else if (flag.chassis_auto_flag == 0 && flag.chassis_handle_flag == 1)
  // {
  //   angle_output = target_angle;
  //   Limit(angle_output, MAX_CHASSIS_ANGLE_SPEED);
  // }
  float motor0 = speed_out_0 + angle_output;
  float motor1 = speed_out_1 + angle_output;
  float motor2 = speed_out_2 + angle_output;
  chassis_canset_motorspeed((int)(motor0), (int)(motor1), (int)(motor2));
}
/**底盘中层驱动(跑向量):now_speed_vec 当前速度;target_speed_vec 目标速度;distance_vec 位移向量; target_angle偏航角*/
vec output;
vec nor;
float vx_output;
float vy_output;
float vector_d = 0;
float x_control = 0.0f;
void chassis_move_vector(vec now_speed_vec, vec target_speed_vec, vec distance_vec, float target_angle)
{
  if (vec_is_zero(target_speed_vec))
  {
    //TODO 目标点速度为0无法运动 需修改 //FIXME:
    chassis_move(0, 0, chassis.angle);
  }
  else
  {

    vector_d = vec_mul(distance_vec, vec_normal(target_speed_vec)); // / vec_model(target_speed_vec); //速度投影
                                                                    //:pid控制量改为distance
                                                                    //vector_d = vec_model(distance_vec);
    vx_output = (float)vec_model(target_speed_vec);                 //法一，直接设置速度
    // vx_output = KalMan(&kal_chassis_x, vx_output);

    vy_output = (float)-PID_Release(&y_pid, 0, vector_d);
    // vy_output = KalMan(&kal_chassis_y, vy_output);

    // uprintf("vx=%f,vy=%f\r\n", vx_output, vy_output);
    // uprintf("x=%f,y=%f\r\n", chassis.pos_x, chassis.pos_y);

    nor = vec_mul_i(vec_normal(target_speed_vec), vy_output);
    output = vec_add(target_speed_vec, nor); //vec_mul_i(vec_normal(target_speed_vec),vy_output));

    chassis.fspeed = (int)vec_model(output);
    chassis.fangle = atan2(output.y, output.x);
    uprintf("VEL: %.4f %.4f\r\n", output.x, output.y);
    chassis_move(chassis.fspeed, chassis.fangle, target_angle);
  }
}
/**底盘上册层驱动(跑轨迹)：points_pos point 轨迹点集; point_num 轨迹点数 */
int chassis_move_trace(Point points_pos[], int point_num)
{
  /*只在每一段路径第一次调用这个函数的时候，赋予chassis_status.point_num初值*/
  if (chassis_status.is_begin == 1)
  {
    chassis_status.point_num = point_num;
    chassis_status.is_begin = 0;
  }
  static vec now_speed_vec, target_speed_vec, distance_vec;
  now_speed_vec = vec_create(chassis.speed_x, chassis.speed_y);
  target_speed_vec = vec_create((float)points_pos[chassis_status.count].speed * cos(points_pos[chassis_status.count].direct),
                                (float)points_pos[chassis_status.count].speed * sin(points_pos[chassis_status.count].direct));
  distance_vec = vec_create(points_pos[chassis_status.count].x - chassis.pos_x, points_pos[chassis_status.count].y - chassis.pos_y);

  double distance_to_next = vec_model(distance_vec);

  float dynamic_arrive_distance = 6 * Arrive_distance; //可修改
  int arriveJudge = (distance_to_next <= Arrive_distance || distance_to_next <= dynamic_arrive_distance);
  if (arriveJudge && (chassis_status.count <= (chassis_status.point_num - 2))) //判断经过此点
  {
    uprintf("%d---(%f,%f,%f)\r\n", chassis_status.count, chassis.pos_x, chassis.pos_y, chassis.angle);
    chassis_status.count += 1;

    // if(chassis_status.count >= chassis_status.point_num - 3){ //到达目的地
    //   chassis_finish_onetrace();
    //   uprintf("到trace%d终点(%f,%f)\r\n",chassis_status.trace_count,chassis.pos_x,chassis.pos_y);
    //   chassis_move(0,0,points_pos[point_num - 1].target_angle);
    //   return 1;
    // }
  }
  if (chassis_status.count < chassis_status.point_num - 3)
  {

    //  _chassis_move_vector_2(now_speed_vec, target_speed_vec, distance_vec, points_pos[chassis_status.count].target_angle);
    chassis_move_vector(now_speed_vec, target_speed_vec, distance_vec, points_pos[chassis_status.count].target_angle);
  }
  /*由于速度慢，最后三个点采用其它跑点方法*/
  else if (chassis_status.count == chassis_status.point_num - 3)
  {
    // chassis_status.trace_count = -1;
    chassis.fangle = chassis_calculate_traceangle(points_pos[chassis_status.count].x, points_pos[chassis_status.count].y);
    chassis.fspeed = chassis_calculate_linespeed(points_pos[chassis_status.count].x, points_pos[chassis_status.count].y, 60, 40, 80);
    chassis_move(chassis.fspeed, chassis.fangle, points_pos[chassis_status.count].target_angle);
  }
  else if (chassis_status.count == chassis_status.point_num - 2)
  {
    //  chassis_status.trace_count = -1;
    chassis.fangle = chassis_calculate_traceangle(points_pos[chassis_status.count].x, points_pos[chassis_status.count].y);
    chassis.fspeed = chassis_calculate_linespeed(points_pos[chassis_status.count].x, points_pos[chassis_status.count].y, 40, 20, 60);
    chassis_move(chassis.fspeed, chassis.fangle, points_pos[chassis_status.count].target_angle);
    // chassis_goto_point(points_pos[chassis_status.count].x, points_pos[chassis_status.count].y); // TODO: 重复?
  }
  else if (chassis_status.count == chassis_status.point_num - 1)
  {
    chassis.fangle = chassis_calculate_traceangle(points_pos[chassis_status.count].x, points_pos[chassis_status.count].y);
    chassis.fspeed = chassis_calculate_linespeed(points_pos[chassis_status.count].x, points_pos[chassis_status.count].y, 20, 0, 30);
    chassis_move(chassis.fspeed, chassis.fangle, points_pos[chassis_status.count].target_angle);
    if (distance_to_next <= Arrive_distance * 1.5)
    {
      uprintf("%d---(%f,%f,%f)\r\n", chassis_status.count, chassis.pos_x, chassis.pos_y, chassis.angle);
      chassis_finish_onetrace();
      return 1;
    }
  }
  return 0;
}

/**底盘顶层驱动(跑全场轨迹)*/
ROUTE_STATUS route_status;
void chassis_move_traces()
{
  // uprintf("s=%d\r\n",route_status);
  switch (route_status)
  {
  case ROUTE_START_TO_LEFT:
    if (chassis_move_trace(points_pos_start_to_left, 151))
    {
      route_status = ROUTE_LEFT_TO_RIGHT;
    }
    break;
  case ROUTE_LEFT_TO_RIGHT:
    if (chassis_move_trace(points_pos_left_to_right, 151))
    {
      route_status = ROUTE_RIGHT_TO_START;
    }
    break;
  case ROUTE_RIGHT_TO_START:
    if (chassis_move_trace(points_pos_right_to_end, 151))
    {
      route_status = ROUTE_NONE;
    }
    break;
  case ROUTE_START_TO_RIGHT:
    chassis_move_trace(points_pos_test, 31);
    break;
  case ROUTE_RIGHT_TO_LEFT:
    chassis_move_trace(points_pos_test, 31);
    break;
  case ROUTE_LEFT_TO_START:
    chassis_move_trace(points_pos_test, 31);
    break;
  case ROUTE_LEFT_ADJUST:
    /* code */
    break;
  case ROUTE_RIGHT_ADJUST:
    /* code */
    break;
  case ROUTE_WAIT:
    break;
  default:
    break;
  }
}

/****************************测试**************************/
/**测试用：随距离减速到某一目标点*/
int chassis_goto_point(float point_x, float point_y, int max_speed)
{
  float distance = sqrtf((chassis.pos_x - point_x) * (chassis.pos_x - point_x) + (chassis.pos_y - point_y) * (chassis.pos_y - point_y));
  if (distance >= Arrive_distance)
  {
    chassis_status.go_to_point = 1;
    chassis.fangle = chassis_calculate_traceangle(point_x, point_y);
    chassis.fspeed = chassis_calculate_linespeed(point_x, point_y, 50, 0, max_speed);
    // uprintf("fspeed = %d, fangle =  %f\r\n", chassis.fspeed, chassis.fangle);
    chassis_move(chassis.fspeed, chassis.fangle, 0);
  }
  else
  {
    chassis_status.go_to_point = 0;
    // uprintf("arrive:%f,%f,%f\r\n", chassis.pos_x, chassis.pos_y, chassis.angle);
    chassis_move(0, 0, chassis.angle);
    return 1;
  }
  return 0;
}
/**测试用：按向量跑*/
void chassis_goto_vector(vec target_position)
{
  chassis.fturn = 0;
  vec distance_vec = vec_create(target_position.x - chassis.pos_x, target_position.y - chassis.pos_y);
  vec target_speed_vec = vec_create(0, 0);
  vec now_speed_vec = vec_create(chassis.speed_x, chassis.speed_y);
  chassis_move_vector(now_speed_vec, target_speed_vec, distance_vec, chassis.angle);
}
/****************************状态&执行**************************/
/**跑完每一段路径后,标志位的改变*/
void chassis_finish_onetrace()
{
  //TODO: 内容待更改
  chassis_status.run_point = 0;
  chassis_status.count = 0;
  chassis.fspeed = 0;
  chassis_status.is_begin = 1; //开始下一段时使用
  flag.chassis_laser_flag = 0;
  // chassis_status.trace_count += 1;
  //测试用
  chassis_status.trace_count = -1;
  // flag.stop_flag = 1;
  // y_pid = (PID_Struct){3000, 125000, 0, 0, 0, 5000, 0, 0.005}; //速度方向控制
  angle_pid = (PID_Struct){1000, 0, 0, 0, 0, 5000, 0, 0.005}; //偏高角控制
}
/**底盘更新坐标*/
void chassis_pos_update()
{
  chassis.pos_x = chassis.vega_pos_x + chassis.vega_init_pos_x;                //m
  chassis.pos_y = chassis.vega_pos_y + chassis.vega_init_pos_y;                //m
  chassis.angle = (chassis.vega_angle / 180.f) * PI + chassis.vega_init_angle; //弧度

  chassis.speed_x = (chassis.pos_x - chassis.last_pos_x) / 0.005;     //m/s
  chassis.speed_y = (chassis.pos_y - chassis.last_pos_y) / 0.005;     //m/s
  chassis.speed_angle = (chassis.angle - chassis.last_angle) / 0.005; //弧度/s
  chassis.now_speed = vec_model(vec_create(chassis.speed_x, chassis.speed_y));

  chassis.last_pos_x = chassis.pos_x;
  chassis.last_pos_y = chassis.pos_y;
  chassis.last_angle = chassis.angle;
}
/**执行函数*/
void chassis_exe()
{
  if (flag.stop_flag)
  {
    if (flag.m3508_flag)
    {
      flag.m3508_flag = 0;
      chassis_canset_motorspeed(0, 0, 0);
    }
    return;
  }
  chassis_pos_update();
  if (flag.chassis_control_flag && flag.m3508_flag)
  {
    flag.m3508_flag = 0;
    // chassis_move_traces();
    // chassis_move_trace(points_pos_start_to_1,151);
    // chassis_goto_point(test_value[0], test_value[1]);
    // if(chassis_move_vector)
  }
}

/**底盘上册层驱动(跑轨迹)：points_pos point 轨迹点集; point_num 轨迹点数 */
int PM_chassis_move_trace(Point points_pos[], int point_num)
{
  /*只在每一段路径第一次调用这个函数的时候，赋予chassis_status.point_num初值*/
  if (chassis_status.is_begin == 1)
  {
    chassis_status.point_num = point_num;
    chassis_status.is_begin = 0;
  }
  static vec now_speed_vec, target_speed_vec, distance_vec;
  now_speed_vec = vec_create(chassis.speed_x, chassis.speed_y);
  target_speed_vec = vec_create((float)points_pos[chassis_status.count].speed * cos(points_pos[chassis_status.count].direct),
                                (float)points_pos[chassis_status.count].speed * sin(points_pos[chassis_status.count].direct));
  target_speed_vec = vec_mul_i(target_speed_vec, test_value[3]); //测试时用来提速
  distance_vec = vec_create(points_pos[chassis_status.count].x - chassis.pos_x, points_pos[chassis_status.count].y - chassis.pos_y);

  double distance_to_next = vec_model(distance_vec);

  float dynamic_arrive_distance = test_value[4] * Arrive_distance; //可修改
  int arriveJudge = (distance_to_next <= Arrive_distance || distance_to_next <= dynamic_arrive_distance);
  if(distance_to_next >= 0.4){   //跑丢直接停下
    chassis_move(0,0,0);
    return 0;
  }
  if (arriveJudge && (chassis_status.count <= (chassis_status.point_num - 2))) //判断经过此点
  {
    uprintf("%d---(%f,%f,%f)\r\n", chassis_status.count, chassis.pos_x, chassis.pos_y, chassis.angle);
    chassis_status.count += 1;
    if (chassis_status.count == chassis_status.point_num - 1)
      return 0; //最后一个点特判
  }
  if (chassis_status.count < chassis_status.point_num - 1)
  {
    //  _chassis_move_vector_2(now_speed_vec, target_speed_vec, distance_vec, points_pos[chassis_status.count].target_angle);
    chassis_move_vector(now_speed_vec, target_speed_vec, distance_vec, points_pos[chassis_status.count].target_angle);
  }
  else if (chassis_status.count == chassis_status.point_num - 1)
  {
    chassis.fangle = chassis_calculate_traceangle(points_pos[chassis_status.count].x, points_pos[chassis_status.count].y);
    chassis.fspeed = chassis_calculate_linespeed(points_pos[chassis_status.count].x, points_pos[chassis_status.count].y, 20, 0, 50);
    chassis_move(chassis.fspeed, chassis.fangle, points_pos[chassis_status.count].target_angle);
    if (distance_to_next <= Arrive_distance)
    {
      uprintf("%d---(%f,%f,%f)\r\n", chassis_status.count, chassis.pos_x, chassis.pos_y, chassis.angle);
      chassis_finish_onetrace();
      return 1;
    }
  }
  return 0;
}

void test_chassis_exe()
{
  if (chassis_start_flag && flag.m3508_flag)
  {
    // uprintf("LASERPOS: %.4f %.4f\r\n", laser_sideRight.distance, laser_right.distance);
    flag.m3508_flag = 0;
    if (go_to_point_flag == 1)
    {
      // laser_go_to_point2((int)test_value[2], 0.10, 0.20, 30);
    }
    else
    {
      chassis_canset_motorspeed(0, 0, 0);
    }
  }
}
void PM_chassis_exe()
{
  chassis_pos_update();
  if (chassis_start_flag && flag.m3508_flag)
  {
    uprintf("POS: %.4f %.4f\r\n", chassis.pos_x, chassis.pos_y);
    flag.m3508_flag = 0;
    if (flag.stop_flag)
    {
      chassis_canset_motorspeed(0, 0, 0);
      return;
    }
    if (flag.chassis_control_flag)
    {
      PM_chassis_move_traces();
    }
  }
}

/**底盘顶层驱动(跑全场轨迹)*/

ROUTE_STATUS PM_route_status;
ROUTE_STATUS next_PM_route_status;
void PM_chassis_move_traces()
{
  // uprintf("s=%d\r\n",route_status);
  switch (PM_route_status)
  {
  case ROUTE_START_TO_LEFT:
    if (PM_chassis_move_trace(points_pos_start_to_left, start_to_left_num))
    {
      PM_route_status = ROUTE_LEFT_ADJUST;
      chassis_init_pos(left_point_x + laser_sideLeft.distance / 100.0 - side_left_laser_data, left_point_y + left_laser_data - laser_left.distance / 100.0);
    }
    break;
  case ROUTE_LEFT_TO_RIGHT:
    if (PM_chassis_move_trace(points_pos_left_to_right, left_to_right_num))
    {
      PM_route_status = ROUTE_RIGHT_ADJUST;
      chassis_init_pos(right_point_x + side_right_laser_data - laser_sideRight.distance / 100.0, right_point_y + right_laser_data - laser_right.distance / 100.0);
    }
    break;
  case ROUTE_RIGHT_TO_START:
    if (PM_chassis_move_trace(points_pos_right_to_end, right_to_end_num))
    {
      PM_route_status = ROUTE_FINISH;
    }
    break;
  case ROUTE_START_TO_RIGHT:
    if (PM_chassis_move_trace(points_pos_start_to_right, start_to_right_num))
    {
      PM_route_status = ROUTE_RIGHT_ADJUST;
      chassis_init_pos(right_point_x + side_right_laser_data - laser_sideRight.distance / 100.0, right_point_y + right_laser_data - laser_right.distance / 100.0);
    }
    break;
  case ROUTE_RIGHT_TO_LEFT:
    if (PM_chassis_move_trace(points_pos_right_to_left, right_to_left_num))
    {
      chassis_init_pos(left_point_x + laser_sideLeft.distance / 100.0 - side_left_laser_data, left_point_y + left_laser_data - laser_left.distance / 100.0);
      PM_route_status = ROUTE_LEFT_ADJUST;
    }
    break;
  case ROUTE_LEFT_TO_START:
    if (PM_chassis_move_trace(points_pos_left_to_end, left_to_end_num))
    {
      PM_route_status = ROUTE_FINISH;
    }
    break;
  case ROUTE_LEFT_ADJUST:
    if (chassis_goto_point(left_PM_point_x, left_PM_point_y, goto_point_speed))
    {
      PM_route_status = ROUTE_LEFT_PM;
      // flag.stop_flag = 1;
    }
    break;
  case ROUTE_RIGHT_ADJUST:
    if (chassis_goto_point(right_PM_point_x, right_PM_point_y, goto_point_speed))
    {
      PM_route_status = ROUTE_RIGHT_PM;
      // flag.stop_flag = 1;
    }
    break;
  case ROUTE_LEFT_PM:
    PM_start_flag = 1;
    PM_num = 1;
    PM_route_status = ROUTE_WAIT;
    if (first_to_go == 1)
    {
      next_PM_route_status = ROUTE_LEFT_TO_RIGHT;
      y_pid = (PID_Struct){3000 * 0.9, 125000 * 0.9, 0, 0, 0, 5000, 0, 0.005}; //reset pid
      chassis_init_pos(points_pos_left_to_right[0].x, points_pos_left_to_right[0].y);
    }
    else if (first_to_go == 3)
    {
      next_PM_route_status = ROUTE_LEFT_TO_START;
      y_pid = (PID_Struct){3000, 125000, 0, 0, 0, 5000, 0, 0.005}; //reset pid
      chassis_init_pos(points_pos_left_to_end[0].x, points_pos_left_to_end[0].y);
    }
    break;
  case ROUTE_RIGHT_PM:
    PM_start_flag = 1;
    PM_num = 3;
    PM_route_status = ROUTE_WAIT;
    if (first_to_go == 1)
    {
      next_PM_route_status = ROUTE_RIGHT_TO_START;
      y_pid = (PID_Struct){3000, 125000, 0, 0, 0, 5000, 0, 0.005}; //reset pid
      chassis_init_pos(points_pos_right_to_end[0].x, points_pos_right_to_end[0].y);
    }
    else if (first_to_go == 3)
    {
      next_PM_route_status = ROUTE_RIGHT_TO_LEFT;
      y_pid = (PID_Struct){3000 * 0.9, 125000 * 0.9, 0, 0, 0, 5000, 0, 0.005}; //reset pid
      chassis_init_pos(points_pos_right_to_left[0].x, points_pos_right_to_left[0].y);
    }
    // next_PM_route_status = ROUTE_FINISH;
    break;
  case ROUTE_WAIT:
    chassis_canset_motorspeed(0, 0, 0);
    if (PM_start_flag == 0)
    {
      PM_route_status = next_PM_route_status;
    }
    break;
  case ROUTE_FINISH:
    chassis_canset_motorspeed(0, 0, 0);
    break;
  default:
    break;
  }
}

/**
 * @brief 只使用全场定位跑
 * 
 */
void PM_chassis_move_traces_planB()
{
  // uprintf("s=%d\r\n",route_status);
  switch (PM_route_status)
  {
  case ROUTE_START_TO_LEFT:
    if (PM_chassis_move_trace(points_pos_start_to_left, 49))
    {
      PM_route_status = ROUTE_LEFT_ADJUST;
    }
    break;
  case ROUTE_LEFT_TO_RIGHT:
    if (PM_chassis_move_trace(points_pos_left_to_right, 61))
    {
      PM_route_status = ROUTE_RIGHT_ADJUST;
    }
    break;
  case ROUTE_RIGHT_TO_START:
    if (PM_chassis_move_trace(points_pos_right_to_end, 49))
    {
      PM_route_status = ROUTE_FINISH;
    }
    break;
  case ROUTE_START_TO_RIGHT:
    PM_chassis_move_trace(points_pos_left_to_right, 31);
    break;
  case ROUTE_RIGHT_TO_LEFT:
    PM_chassis_move_trace(points_pos_left_to_right, 31);
    break;
  case ROUTE_LEFT_TO_START:
    PM_chassis_move_trace(points_pos_left_to_right, 31);

    break;
  case ROUTE_LEFT_ADJUST:
    PM_route_status = ROUTE_LEFT_PM;
    break;
  case ROUTE_RIGHT_ADJUST:
    PM_route_status = ROUTE_RIGHT_PM;
    break;
  case ROUTE_LEFT_PM:
    PM_start_flag = 1;
    PM_num = 1;
    PM_route_status = ROUTE_WAIT;
    next_PM_route_status = ROUTE_LEFT_TO_RIGHT;
    break;
  case ROUTE_RIGHT_PM:
    PM_start_flag = 1;
    PM_num = 3;
    PM_route_status = ROUTE_WAIT;
    next_PM_route_status = ROUTE_RIGHT_TO_START;
    break;
  case ROUTE_WAIT:
    chassis_canset_motorspeed(0, 0, 0);
    if (PM_start_flag == 0)
    {
      PM_route_status = next_PM_route_status;
    }
    break;
  case ROUTE_FINISH:
    chassis_canset_motorspeed(0, 0, 0);
    break;
  default:
    break;
  }
}

#define LASER_LEFT_RIGHT_DIFFER 40
const static float x_speed = 50;
float left_route_front_distance = 15;
float right_route_front_distance = 15;
/**
 * @brief 使用激光突变作为判定标志
 * 
 */
void PM_chassis_move_traces_planC()
{
  // uprintf("s=%d\r\n",route_status);
  switch (PM_route_status)
  {
  case ROUTE_START_TO_LEFT:
    if (PM_chassis_move_trace(points_pos_start_to_left, 49))
    {
      PM_route_status = ROUTE_LEFT_ADJUST;
    }
    break;
  case ROUTE_LEFT_TO_RIGHT:
    if (PM_chassis_move_trace(points_pos_left_to_right, 61))
    {
      PM_route_status = ROUTE_RIGHT_ADJUST;
    }
    break;
  case ROUTE_RIGHT_TO_START:
    if (PM_chassis_move_trace(points_pos_right_to_end, 49))
    {
      PM_route_status = ROUTE_FINISH;
    }
    break;
  case ROUTE_START_TO_RIGHT:
    PM_chassis_move_trace(points_pos_start_to_right, 49);
    break;
  case ROUTE_RIGHT_TO_LEFT:
    PM_chassis_move_trace(points_pos_right_to_left, 61);
    break;
  case ROUTE_LEFT_TO_START:
    PM_chassis_move_trace(points_pos_left_to_end, 49);

    break;
  case ROUTE_LEFT_ADJUST:

    if (laser_right.distance - laser_left.distance > LASER_LEFT_RIGHT_DIFFER)
      PM_route_status = ROUTE_LEFT_PM;
    else
    {
      float distance_y = (laser_left.distance - left_route_front_distance) / 100;
      float y_speed = test_value[0] * distance_y;
      chassis.fspeed = (int)(sqrt(x_speed * x_speed + y_speed * y_speed));
      chassis.fangle = atan2f(y_speed, x_speed);
      chassis_move(chassis.fspeed, chassis.fangle, 0);
    }
    break;
  case ROUTE_RIGHT_ADJUST:
    if (laser_left.distance - laser_right.distance > LASER_LEFT_RIGHT_DIFFER)
      PM_route_status = ROUTE_RIGHT_PM;
    else
    {
      // chassis_move(x_speed), PI, 0);
      // y轴使用goto_point算法稳定位置
    }
    break;
  case ROUTE_LEFT_PM:
    PM_start_flag = 1;
    PM_num = 1;
    PM_route_status = ROUTE_WAIT;
    next_PM_route_status = ROUTE_LEFT_TO_RIGHT;
    break;
  case ROUTE_RIGHT_PM:
    PM_start_flag = 1;
    PM_num = 3;
    PM_route_status = ROUTE_WAIT;
    next_PM_route_status = ROUTE_RIGHT_TO_START;
    break;
  case ROUTE_WAIT:
    chassis_canset_motorspeed(0, 0, 0);
    if (PM_start_flag == 0)
    {
      PM_route_status = next_PM_route_status;
    }
    break;
  case ROUTE_FINISH:
    chassis_canset_motorspeed(0, 0, 0);
    break;
  default:
    break;
  }
}